/* spectral_div.c */

#include "spectral_div.h"
#include <math.h>
#include <stdlib.h>
#include <memory.h>

void compute_s(elem* s, int* g, elem* eigenvector, int size)
{
	int i = 0;
	
	if (size > 0)
	{
		for (i = 0; i < size; ++i)
		{
			if (IS_POSITIVE(eigenvector[i]))
			{
				s[i] = 1;
			}
			else
			{
				s[i] = -1;
			}
		}
	}
}

void mult_vector_matrix(elem* result, elem* vector, elem** matrix, int size)
{
	int j = 0, i = 0;
	
	if (size > 0)
	{
		for (i = 0; i < size; ++i)
		{
			for(j = 0; j < size; ++j)
			{
				result[j] += matrix[i][j] * vector[i];
			}
		}
	}
}

elem mult_vector_vector(elem* vector_a, elem* vector_b, int size)
{
	int i = 0;
	elem res = 0;

	if (size > 0)
	{	
		for(i = 0; i < size; ++i)
		{
			res += vector_a[i] * vector_b[i];
		}
	}
	return res;
}

void mult_matrix_vector(elem* result, elem** mat, elem* vec, int size)
{
	int j = 0, i = 0;
	elem sum = 0;

	if (size > 0)
	{
		for (i = 0; i < size; ++i)
		{
			sum = 0;
			for(j = 0; j < size; ++j)
			{
				sum += mat[i][j] * vec[j];
			}
			result[i] = sum;
		}
	}
}

void vectors_sum (elem* result, elem* vec_a, elem* vec_b, int size)
{
	int i = 0;

	for (i = 0; i < size; ++i)
	{
		result[i] = vec_a[i] + vec_b[i];
	}
}

void vectors_difference (elem* result, elem* vec_a, elem* vec_b, int size)
{
	int i = 0;

	for (i = 0; i < size; ++i)
	{
		result[i] = vec_a[i] - vec_b[i];
	}
}

void mult_vector_scalar(elem* result, elem* vector, elem scalar, int size)
{
	int i = 0;

	for (i = 0; i < size; ++i)
	{
		result[i] = vector[i] * scalar;
	}
}

sparse_matrix_lst* create_A_g(sparse_matrix_lst* A, int* g, int elements_no)
{
	int i = 0, j = 0;
	sparse_matrix_lst* result = NULL;
	cell_t *current_cell = NULL, *A_g_cell = NULL;
	cell_t *prev_row_cell = NULL, *prev_col_cell = NULL;

	result = allocate_sparse_matrix_lst(elements_no);

	for(i = 0; i < elements_no; i++)
	{
		j=0;
		prev_row_cell = NULL;
		current_cell = A->rows[g[i]];
		while(current_cell != NULL && j < elements_no)
		{
			while(current_cell != NULL && current_cell->colind < g[j])
			{
				current_cell = current_cell->col_next;
			}
			if (current_cell != NULL && current_cell->colind == g[j])
			{
				A_g_cell = (cell_t*)malloc(sizeof(cell_t));
				A_g_cell->col_next = NULL;
				A_g_cell->row_next = NULL;
				A_g_cell->colind = j;
				A_g_cell->rowind = i;
				A_g_cell->value = current_cell->value;

				if (prev_row_cell == NULL)
				{
					prev_row_cell = A_g_cell;
					result->rows[i] = A_g_cell;
				}
				else
				{
					prev_row_cell->col_next = A_g_cell;
					prev_row_cell = A_g_cell;
				}

				/* Finding the previous column */
				prev_col_cell = result->cols[j];
				if (prev_col_cell == NULL)
				{
					prev_col_cell = A_g_cell;
					result->cols[j] = A_g_cell;
				}
				else
				{
					while(prev_col_cell->row_next != NULL)
					{
						prev_col_cell = prev_col_cell->row_next;
					}
					prev_col_cell->row_next = A_g_cell;
				}
			}
			++j;
		}
	}
	return result;
}

elem* create_X(int n)
{
	elem* X = NULL;
	int i = 0;

	X = (elem*)calloc(n, sizeof(elem));
	assert(X != NULL);

	for (i = 0; i < n; ++i)
	{
		X[i] = (elem)(rand()%50);
		X[i] += 1;
	}

	return X;
}

sparse_matrix_lst* create_D(elem* f, int elements_no)
{
	sparse_matrix_lst *D = NULL;
	int i = 0;
	cell_t* new_element = NULL;

	D = allocate_sparse_matrix_lst(elements_no);

	for (i = 0; i < elements_no; ++i)
	{
		new_element = (cell_t*)malloc(sizeof(cell_t));
		new_element->value = f[i];
		new_element->colind = i;
		new_element->rowind = i;
		new_element->col_next = NULL;
		new_element->row_next = NULL;
		D->rows[i] = new_element;
		D->cols[i] = new_element;
	}

	return D;
}

elem* create_k_g(elem* k, int* g, int elements_no)
{
	elem* k_g = NULL;
	int i = 0;

	k_g = (elem*)calloc(elements_no, sizeof(elem));
	assert(k_g != NULL);

	for (i = 0; i < elements_no; ++i)
	{
		k_g[i] = k[g[i]];
	}

	return k_g;
}

elem calc_norm_1(elem** gen_B_g, int elements_no)
{
	elem sum = 0, max_sum = 0;
	int i = 0, j = 0;

	/* For each column, going over all rows */
	for (j = 0; j < elements_no; ++j)
	{
		sum = 0;
		for (i = 0; i < elements_no; ++i)
		{
			sum += fabs(gen_B_g[i][j]);
		}
		if (sum > max_sum)
		{
			max_sum = sum;
		}
	}

	return max_sum;
}

elem calculate_eigenvalue(elem* Xm, elem* Xm_1, int elements_no)
{
	elem result = mult_vector_vector(Xm, Xm_1, elements_no) / 
							mult_vector_vector(Xm, Xm, elements_no);

	return result;
}

void fast_calculation(elem* result, int* g, elem* X, sparse_matrix_lst* A_g,
					  sparse_matrix_lst* D, elem* k_g, elem M,
					  elem norm_1_gen_B, int elements_no)
{
	elem *D_g_X = NULL, *norm_B_X = NULL, *k_g_x_k_M = NULL;
	elem k_g_x, k_g_x_M;
	
	mult_sparse_list(A_g, X, result);
	k_g_x = mult_vector_vector(k_g, X, elements_no);
	k_g_x_M = k_g_x / M;

	k_g_x_k_M = (elem*)calloc(elements_no, sizeof(elem));
	assert(k_g_x_k_M != NULL);

	mult_vector_scalar(k_g_x_k_M, k_g, k_g_x_M, elements_no);

	D_g_X = (elem*)calloc(elements_no, sizeof(elem));
	if (D_g_X == NULL)
	{
		goto fast_calculation_cleanup;
	}

	mult_sparse_list(D, X, D_g_X);

	norm_B_X = (elem*)calloc(elements_no, sizeof(elem));
	assert(norm_B_X != NULL);

	mult_vector_scalar(norm_B_X, X, norm_1_gen_B, elements_no);

	vectors_difference(result, result, k_g_x_k_M, elements_no);
	vectors_difference(result, result, D_g_X, elements_no);

	vectors_sum(result, result, norm_B_X, elements_no);
	
fast_calculation_cleanup:
	if (D_g_X != NULL)
		free(D_g_X);
	if (norm_B_X != NULL)
		free(norm_B_X);
	if (k_g_x_k_M != NULL)
		free(k_g_x_k_M);
}

elem geometric_length(elem* v, int n)
{
	elem length = 0;
	if (n > 0) 
	{
		int i = 0;
		for (i = 0; i < n; ++i)
		{
			length += v[i]*v[i];
		}

		length = sqrt(length);
	}

	return length;
}

void normalize_vector(elem* v, int n)
{
	int i = 0;
	elem max_value = 0;

	if (n > 0)
	{
		max_value = fabs(v[0]);
		for (i = 1; i < n; ++i)
		{
			max_value = max(max_value, fabs(v[i]));
		}

		for (i = 0; i < n; ++i)
		{
			v[i] /= max_value;
		}
	}
}

int requirement_met(elem* Xm, elem* Xm_1, elem precision, elem lambda, int n)
{
	elem *lambda_Xm = NULL;
	elem left, right;

	right = geometric_length(Xm, n);

	lambda_Xm = (elem*)calloc(n, sizeof(elem));
	assert(lambda_Xm != NULL);
	mult_vector_scalar(lambda_Xm, Xm, lambda, n);
	vectors_difference(lambda_Xm, Xm_1, lambda_Xm, n);

	left = geometric_length(lambda_Xm, n);
	
	free(lambda_Xm);

	return(left <= precision * right);
}

eigenpair find_eigenpair(sparse_matrix_lst* A_g, elem** gen_B_g,
						 elem* k_g, elem* f, elem M, int* g,
						 int elements_no, elem precision)
{
	elem *Xm = NULL, *Xm_1 = NULL, *tmpXm = NULL;
	sparse_matrix_lst* D = NULL;
	elem norm_1_gen_B = 0;
	elem lambda = 0;
	eigenpair result;

	Xm = create_X(elements_no);
	normalize_vector(Xm, elements_no);
	/* Calculating the parts */
	D = create_D(f, elements_no);
	
	norm_1_gen_B = calc_norm_1(gen_B_g, elements_no);

	Xm_1 = (elem*)calloc(elements_no, sizeof(elem));
	fast_calculation(Xm_1, g, Xm, A_g, D, k_g, M, norm_1_gen_B, elements_no);

	lambda = calculate_eigenvalue(Xm, Xm_1, elements_no);
	while(!requirement_met(Xm, Xm_1, precision, lambda, elements_no))
	{
		tmpXm = Xm;
		Xm = Xm_1;
		normalize_vector(Xm, elements_no);
		Xm_1 = tmpXm;

		fast_calculation(Xm_1, g, Xm, A_g, D, k_g, M, norm_1_gen_B,
						 elements_no);
		lambda = calculate_eigenvalue(Xm, Xm_1, elements_no);
	}
	
	/* Compensate for matrix shifting */
	result.eigenvalue = lambda - norm_1_gen_B;
	result.eigenvector = Xm_1;

	if (Xm != NULL)
		free(Xm);
	if (D != NULL)
		free_sparse_matrix_lst(D);

	return result;
}


void div_indices(elem* s, int* g, int** g1, int** g2, int* g1_size,
				int* g2_size, int size)
{
	int i = 0, g1_i = 0, g2_i = 0;
	*g1_size = 0;
	*g2_size = 0;
	for (i = 0; i < size; ++i)
	{
		(s[i] > 0) ? ++(*g1_size) : ++(*g2_size);
	}

	*g1 = (int*)calloc(*g1_size, sizeof(int));
	assert(g1 != NULL);

	*g2 = (int*)calloc(*g2_size, sizeof(int));
	assert(g2 != NULL);

	for (i = 0; i < size; ++i)
	{
		if (s[i] > 0)
		{
			(*g1)[g1_i] = g[i];
			++g1_i;
		}
		else
		{
			(*g2)[g2_i] = g[i];
			++g2_i;
		}
	}
}

void print_groups(int* g1, int* g2, int g1_size, int g2_size)
{
	if (g2[0] < g1[0])
	{
		print_group(g2, g2_size);
		printf("\n");
		print_group(g1, g1_size);
	} 
	else
	{
		print_group(g1, g1_size);
		printf("\n");
		print_group(g2, g2_size);
	}
}


void print_group (int* g, int size)
{
	int i = 0;
	for (i = 0; i < size; ++i)
	{
		printf("%d ", g[i]);
	}
}
